// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ManipulationGamePadDriveAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.Runtime
{
    using System;
    using System.Diagnostics;
    using System.Runtime.Serialization;
    using System.Threading;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// Use game pad to send arm control messages
    /// </summary>
    [DataContract]
    public class ManipulationGamePadDriveAgent : ConsumerAgent<GamePadAgentMessage>,
                                                ISubscriptionReceiver<ManipulatorStatusMessage>
    {
        /// <summary>
        /// Default scaling in any direction
        /// </summary>
        private const double DefaultScale = 0.1;

        /// <summary>
        /// Last Cartesian position
        /// </summary>
        private Pose lastCartesianArmPosition;

        /// <summary>
        /// Last joint positions
        /// </summary>
        private double[] lastJointArmPosition;

        /// <summary>
        /// scaling in X direction for left thumb stick
        /// </summary>
        [DataMember(Name = "xScaleLeft")]
        private double xScaleLeft;

        /// <summary>
        /// scaling in Y direction for left thumb stick
        /// </summary>
        [DataMember(Name = "yScaleLeft")]
        private double yScaleLeft;

        /// <summary>
        /// scaling in X direction for right thumb stick
        /// </summary>
        [DataMember(Name = "xScaleRight")]
        private double xScaleRight;

        /// <summary>
        /// scaling in Y direction for right thumb stick
        /// </summary>
        [DataMember(Name = "yScaleRight")]
        private double yScaleRight;

        /// <summary>
        /// Name of the manipulation agent
        /// </summary>
        [DataMember(Name = "ManipulationAgentName")]
        private string manipulationAgentName = default(string);

        /// <summary>
        /// Maximum cartesian move error tolerance
        /// </summary>
        [DataMember(Name = "CartesianMoveTolerance")]
        private double cartesianMoveTolerance;

        /// <summary>
        /// Maximum joint move error tolerance
        /// </summary>
        [DataMember(Name = "JointMoveTolerance")]
        private double jointMoveTolerance;

        /// <summary>
        /// Control port for arm cartesian control
        /// </summary>
        private IForwarder<LinearCartesianMotionControlMessage> armCartesianControlPort;

        /// <summary>
        /// Control port for arm cartesian control
        /// </summary>
        private IForwarder<JointGoalPositionControlMessage> armJointControlPort;

        /// <summary>
        /// Maximum increment on left joystick
        /// </summary>
        private double maxLeftScale;

        /// <summary>
        /// Initializes a new instance of the <see cref="ManipulationGamePadDriveAgent" /> class
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="producer">Message producer</param>
        /// <param name="manipulationAgentName">manipulation control agent</param>
        /// <param name="xScaleLeft">Scale factors for left X thumb stick</param>
        /// <param name="yScaleLeft">Scale factor for right Y thumb stick</param>
        /// <param name="xScaleRight">Scale factors for right X thumb stick</param>
        /// <param name="yScaleRight">Scale factors for right Y thumb stick</param>
        /// <param name="cartesianControlTolerance">cartesian move tolerance</param>
        /// <param name="jointControlTolerance">joint move tolerance</param>
        public ManipulationGamePadDriveAgent(
                    string name,
                    IProducer<GamePadAgentMessage> producer,
                    string manipulationAgentName,
                    double xScaleLeft,
                    double yScaleLeft,
                    double xScaleRight,
                    double yScaleRight,
                   double cartesianControlTolerance,
                   double jointControlTolerance)

            : base(name, producer.Name)
        {
            this.manipulationAgentName = manipulationAgentName;
            this.cartesianMoveTolerance = cartesianControlTolerance;
            this.xScaleLeft = xScaleLeft;
            this.yScaleLeft = yScaleLeft;
            this.jointMoveTolerance = jointControlTolerance;
            this.xScaleRight = xScaleRight;
            this.yScaleRight = yScaleRight;
        }

        /// <summary>
        /// Initializes the agent
        /// </summary>
        /// <param name="locator">Agent locator to use</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
            if (false == string.IsNullOrEmpty(this.manipulationAgentName))
            {
                IAgentHost manipulatorAgent = locator.GetAgent(this.manipulationAgentName);
                manipulatorAgent.Subscribe<ManipulatorStatusMessage>(this);
                this.armCartesianControlPort = manipulatorAgent.GetControlPort<LinearCartesianMotionControlMessage>();
                this.armJointControlPort = manipulatorAgent.GetControlPort<JointGoalPositionControlMessage>();
            }
            else
            {
                throw new ArgumentException("No manipulation agent name set so cannot a port to send arm control messages");
            }

            if (this.xScaleLeft == default(double))
            {
                this.xScaleLeft = ManipulationGamePadDriveAgent.DefaultScale;
            }

            if (this.yScaleLeft == default(double))
            {
                this.yScaleLeft = ManipulationGamePadDriveAgent.DefaultScale;
            }

            this.maxLeftScale = Math.Sqrt(this.xScaleLeft * this.xScaleLeft + this.yScaleLeft * this.yScaleLeft);
        }

        /// <summary>
        /// Called when the agent is about to start running
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();
            this.lastCartesianArmPosition = new Pose();
        }

        /// <summary>
        /// Handles the game pad message
        /// </summary>
        /// <param name="message">Game pad message</param>
        public override void Receive(GamePadAgentMessage message)
        {
            if ((0 != message.ThumbSticks.Left.X || 0 != message.ThumbSticks.Left.Y) && null != this.lastCartesianArmPosition)
            {
                Pose last = this.lastCartesianArmPosition;
                last.Position = new Vector3(
                    last.Position.X + message.ThumbSticks.Left.X * this.xScaleLeft,
                last.Position.Y + message.ThumbSticks.Left.Y * this.yScaleLeft,
                last.Position.Z);

                this.lastCartesianArmPosition = last;
                LinearCartesianMotionControlMessage msg = new LinearCartesianMotionControlMessage(message.OriginatingTime, last, this.cartesianMoveTolerance);
                this.armCartesianControlPort.Post(msg);
            }

            if ((0 != message.ThumbSticks.Right.X || 0 != message.ThumbSticks.Right.Y) && null != this.lastJointArmPosition)
            {
                // Quick and dirty up date for up to 8 joint angles
                // Divide the (X,Y) joy stick plane into 8 quadrants by angle. the joints bounding
                // the angle are updated by projecting the actual (X,Y) onto the boundaries of the quadrant
                // Example: Say the joy stick is in quadrant 1 that is  45' <= angle < 90' then (zero based)
                // joint angles 1 and 2 are updated as:
                //  angle1 += mag * (1-alpha)
                //  angle2 += mag * alpha
                // where alpha = (angle - baseQuad*45degreeAsRadian) / 45degreesAsRadians
                // One extra complication when projecting the zero is set at half the maximum magnitude
                // scale So that small joystick movements decrease the chosen joint , larger increase it
                double x = message.ThumbSticks.Right.X * this.xScaleRight;
                double y = message.ThumbSticks.Right.Y * this.yScaleRight;
                double max;
                double theta = Math.Atan2(message.ThumbSticks.Right.Y, message.ThumbSticks.Right.X);
                if (Math.Abs(message.ThumbSticks.Right.X) > Math.Abs(message.ThumbSticks.Right.Y))
                {
                    max = this.xScaleRight / Math.Abs(Math.Cos(theta));
                }
                else
                {
                    max = this.yScaleRight / Math.Abs(Math.Sin(theta));
                }

                double mag = Math.Sqrt(x * x + y * y) - max / 2.0;
                double angle = Math.Atan2(y, x);
                
                // Recast angle into range 0 -> 2PI
                angle = (angle < 0) ? MathConstants.TwoPI + angle : angle;
                double facPiBy4 = Math.PI / 4.0;
                int baseQuad = (int)Math.Floor(angle / facPiBy4);

                // Corner case map 2PI -> 0
                if (baseQuad >= 8)
                {
                    baseQuad = 0;
                }

                double alpha = (angle - baseQuad * facPiBy4) / facPiBy4;

                // Last quadrant wraps around to quadrant 0
                int baseQuad1 = baseQuad < 7 ? baseQuad + 1 : 0;

                if (baseQuad < this.lastJointArmPosition.Length)
                {
                    this.lastJointArmPosition[baseQuad] += mag * (1.0 - alpha);
                    this.lastJointArmPosition[baseQuad] =
                                    (Math.Abs(this.lastJointArmPosition[baseQuad]) > Math.PI) ?
                                    this.lastJointArmPosition[baseQuad] - Math.Sign(this.lastJointArmPosition[baseQuad]) * MathConstants.TwoPI :
                                    this.lastJointArmPosition[baseQuad];
                }

                if (baseQuad1 < this.lastJointArmPosition.Length)
                {
                    this.lastJointArmPosition[baseQuad1] += mag * alpha;
                    this.lastJointArmPosition[baseQuad1] =
                                    (Math.Abs(this.lastJointArmPosition[baseQuad1]) > Math.PI) ?
                                    this.lastJointArmPosition[baseQuad1] - Math.Sign(this.lastJointArmPosition[baseQuad1]) * MathConstants.TwoPI :
                                    this.lastJointArmPosition[baseQuad1];
                }

                JointGoalPositionControlMessage msg = new JointGoalPositionControlMessage(message.OriginatingTime, this.lastJointArmPosition, this.jointMoveTolerance);
                this.armJointControlPort.Post(msg);
            }
        }

        /// <summary>
        /// Handles reception of the status message
        /// </summary>
        /// <param name="message">Status message</param>
        public void Receive(ManipulatorStatusMessage message)
        {
            this.lastCartesianArmPosition = message.State.TcpCartesianPosition;
            this.lastJointArmPosition = message.State.JointPositions;
        }
    }
}
